#include "CMASearchContinuation.h"
#include "CMASearcher.h"
#include "Scene.h"
#include "SceneBuilder.h"
#include "DecoArticulatedBody.h"
#include "DecoLogger.h"
#include "cmaes_interface.h"
#include "SearcherConfig.h"
#include "ObjectiveFunctionCollection.h"

double gPassingVariables[100];
double (*gEvaluator)(double*, DecoScene*, double*);

double ObjectiveEvaluator(DecoArticulatedObject* articulatedObj, double* resultDetails)
{
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
	switch (info.mObjFuncId)
	{
	case 0:
		return GoStraightWithEnergyConstraints(articulatedObj, resultDetails);
	case 1:
		return TurnCircularWithEnergyConstraints(articulatedObj, resultDetails);
	case 2:
		return TurnRightWithEnergyConstraints(articulatedObj, resultDetails);
	case 3:
		return GoStraightAllowRollWithEnergyConstraints(articulatedObj, resultDetails);
	case 4:
		return PitchUpWithEnergyConstraints(articulatedObj, resultDetails);
	default:
		return 0.0;


	}
}

double Evaluator(double* params, DecoScene* scene, double* resultDetails)
{
    if (scene)
		SceneBuilder::GetSingleton()->ConstructSceneWithoutDestroy(scene);
    else
      return 1e6;

	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
  
    DecoArticulatedObject* articulatedObj = scene->GetArticulatedObject();
    //double params1[] = {0.129767,  0.490175,  0.687749,  0.861621,  4.986412,  3.357822,  2.139741 };
    //articulatedObj->SetMotionScriptAmplitude("linka_expZ", params[0]);
    //articulatedObj->SetMotionScriptAmplitude("linkb_expZ", params[1]);
    //articulatedObj->SetMotionScriptAmplitude("linkc_expZ", params[2]);
   // articulatedObj->SetMotionScriptAmplitude("linkd_expZ", params[3]);


    //articulatedObj->SetMotionScriptPhase("linka_expZ", 0);
    //articulatedObj->SetMotionScriptPhase("linkb_expZ", params[3]);
    //articulatedObj->SetMotionScriptPhase("linkc_expZ", params[4]);
    //articulatedObj->SetMotionScriptPhase("linkd_expZ", params[6]);
	double* newParams = NULL;
	if (info.mJointParams.size() != 0)
	{
		int numKnownParams = static_cast<int>(info.mJointParams.size());
		int searchDim = static_cast<int>(info.mJointBounds.size() - info.mJointParams.size());

		newParams = new double[info.mJointBounds.size()];
		for (int i = 0; i < numKnownParams; ++i)
		{
			newParams[i] = info.mJointParams[i];
		}

		for (int i = 0; i < searchDim; ++i)
		{
			newParams[i + numKnownParams] = params[i];
		}
		articulatedObj->ApplyMotion(info, newParams, static_cast<int>(info.mJointBounds.size()));
	}
	else
	{
		articulatedObj->ApplyMotion(info, params, static_cast<int>(info.mJointBounds.size()));
	}

	//need modification tanjie

	DOUBLE TIME_STEP = scene->GetTimeStep();

	int ithFrame = 0;
	while (true)
	{
		if (scene->GetCurrentTime() >= info.mSearchTimeLength)
			break;
		if(scene->GetCurrentTime() >= 2.0 && articulatedObj->mPosFirstCycle == vector3(-999, -999, -999))
		{
			std::vector<double> q;
			articulatedObj->GetQ(q);
			articulatedObj->mPosFirstCycle = vector3(q[0], q[1], q[2]);
			articulatedObj->mOriFirstCycle = vector3(q[3], q[4], q[5]);

			DecoSkelMesh* pMesh = (DecoSkelMesh*)articulatedObj->GetRenderData();
			DecoStaticMesh* pBody = (DecoStaticMesh*)pMesh->GetRenderDataFromIthBody(0);
			vector4 vDir4 = pBody->GetLocalToWorld() * vector4(-1.0, 0.0, 0.0, 0.0);
			articulatedObj->mDirFirstCycle = vector3(vDir4.x, vDir4.y, vDir4.z);
			articulatedObj->mDirFirstCycle.normalize();
		}
		if (info.mbEnergyPrune)
		{ 
			double energy = articulatedObj->GetCumulativeEnergyConsumption().x;
			if (energy > 1.5 * info.mEnergyBound)
			{
				if (newParams)
					delete[] newParams;
				double objFuncValue = energy * info.mSearchTimeLength / scene->GetCurrentTime();
				if (resultDetails)
					resultDetails[0] = objFuncValue;

				printf("Energy pruned: %f %f\n", energy, objFuncValue);
				return objFuncValue;
			}

			for(int i = 0; i < articulatedObj->GetNumBodies(); i++)
			{
				if(info.mIdxEnergyBounds[i] == 0.0)
					continue;

				if(articulatedObj->mCumulativeIndexedEnergy[i] > 1.5 * info.mIdxEnergyBounds[i])
				{
					if (newParams)
						delete[] newParams;
					double objFuncValue = articulatedObj->mCumulativeIndexedEnergy[i] * info.mSearchTimeLength / scene->GetCurrentTime();
					if (resultDetails)
						resultDetails[0] = objFuncValue;

					printf("%dth body energy pruned: %f %f\n", i, articulatedObj->mCumulativeIndexedEnergy[i], objFuncValue);
					return objFuncValue;
				}
			}

		}
		DecoTimer timer;
		timer.startTimer();
		scene->Update(TIME_STEP);
		double currentTime = timer.getCurrentTime();
		(*DecoLogger::GetSingleton()) << ithFrame << "th frame spent " << currentTime << " seconds.\n";
		//	printf("%dth frame takes %f seconds\n", ithFrame, currentTime);
		ithFrame++;
	}
	if (newParams)
		delete[] newParams;

	return ObjectiveEvaluator(articulatedObj, resultDetails);

}

double Evaluator2(double* params, DecoScene* scene, double* resultDetails)
{
    if (scene)
	SceneBuilder::GetSingleton()->ConstructSceneWithoutDestroy(scene);
    else
      return 1e6;

    DecoArticulatedObject* articulatedObj = scene->GetArticulatedObject();



    articulatedObj->SetMotionScriptAmplitude("linka_expZ", gPassingVariables[0]);
    articulatedObj->SetMotionScriptAmplitude("linkb_expZ", -gPassingVariables[0]);
    articulatedObj->SetMotionScriptAmplitude("linkc_expZ", gPassingVariables[1]);
    articulatedObj->SetMotionScriptAmplitude("linkd_expZ", -gPassingVariables[1]);
    articulatedObj->SetMotionScriptAmplitude("linke_expZ", gPassingVariables[2]);
    articulatedObj->SetMotionScriptAmplitude("linkf_expZ", -gPassingVariables[2]);

    articulatedObj->SetMotionScriptPhase("linka_expZ", 0);
    articulatedObj->SetMotionScriptPhase("linkb_expZ", 0);
    articulatedObj->SetMotionScriptPhase("linkc_expZ", gPassingVariables[3]);
    articulatedObj->SetMotionScriptPhase("linkd_expZ", gPassingVariables[3]);
    articulatedObj->SetMotionScriptPhase("linke_expZ", gPassingVariables[4]);
    articulatedObj->SetMotionScriptPhase("linkf_expZ", gPassingVariables[4]);

    //articulatedObj->SetMotionScriptConstant("linka_expZ", gPassingVariables[0]);
    //articulatedObj->SetMotionScriptConstant("linkb_expZ", -gPassingVariables[0]);
    //articulatedObj->SetMotionScriptConstant("linkc_expZ", gPassingVariables[1]);
    //articulatedObj->SetMotionScriptConstant("linkd_expZ", -gPassingVariables[1]);


    std::vector<double> ctrlPts;

    ctrlPts.assign(params + 0, params + 4);
    articulatedObj->SetMotionScriptSineWarpControlPts("linka_expZ", ctrlPts);
    articulatedObj->SetMotionScriptSineWarpControlPts("linkb_expZ", ctrlPts);

    ctrlPts.assign(params + 4, params + 8);
    articulatedObj->SetMotionScriptSineWarpControlPts("linkc_expZ", ctrlPts);
    articulatedObj->SetMotionScriptSineWarpControlPts("linkd_expZ", ctrlPts);

    ctrlPts.assign(params + 8, params + 12);
    articulatedObj->SetMotionScriptSineWarpControlPts("linke_expZ", ctrlPts);
    articulatedObj->SetMotionScriptSineWarpControlPts("linkf_expZ", ctrlPts);

    //need modification tanjie

    DOUBLE TIME_STEP = scene->GetTimeStep();

    int ithFrame = 0;
    while (true)
    {
	if (scene->GetCurrentTime() >= 2)
	    break;

	DecoTimer timer;
	timer.startTimer();
	scene->Update(TIME_STEP);
	double currentTime = timer.getCurrentTime();
	(*DecoLogger::GetSingleton()) << ithFrame << "th frame spent " << currentTime << " seconds.\n";
	ithFrame++;
    }
    return ObjectiveEvaluator(articulatedObj, resultDetails);
}

CMASearchContinuation::CMASearchContinuation()
{

}

CMASearchContinuation::~CMASearchContinuation()
{

}

void CMASearchContinuation::Search()
{
    searchFirstPass();
    //searchSecondPass();
}

void CMASearchContinuation::getBoundaryFirstPass(double* minBoundary, double* maxBoundary, int searchDim)
{
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
	assert(searchDim == info.mJointBounds.size());
	for (int i = 0; i < searchDim; ++i)
	{
		minBoundary[i] = info.mJointBounds[i].mMinValue;
		maxBoundary[i] = info.mJointBounds[i].mMaxValue;
	}
}

void CMASearchContinuation::getBoundarySecondPass(double* minBoundary, double* maxBoundary, int searchDim)
{
}

void CMASearchContinuation::searchFirstPass()
{
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();

    int searchDim = static_cast<int>(info.mJointBounds.size());

    double* minBoundary = new double[searchDim];
    double* maxBoundary = new double[searchDim];

	getBoundaryFirstPass(minBoundary, maxBoundary, searchDim);
	int numParamAssigned = static_cast<int>(info.mJointParams.size());
	if (numParamAssigned >= searchDim)
	{
		printf("All the parameters are already assgined, no search needed\n");
		(*DecoLogger::GetSingleton()) << "All the parameters are already assgined, no search needed\n";
		delete[] minBoundary;
		delete[] maxBoundary;
		return;
	}
    CMASearcher searcher(searchDim, numParamAssigned);
    gEvaluator = &Evaluator;
    searcher.Search(&minBoundary[numParamAssigned], &maxBoundary[numParamAssigned], gPassingVariables, 100);
    delete[] minBoundary;
    delete[] maxBoundary;
}

void CMASearchContinuation::searchSecondPass()
{
    const int searchDim = 12;
    CMASearcher searcher(searchDim, 0);

    double* minBoundary = new double[searchDim];
    double* maxBoundary = new double[searchDim];

    minBoundary[0] = 0.0;
    minBoundary[1] = 0.25;
    minBoundary[2] = 0.5;
    minBoundary[3] = 0.75;

    maxBoundary[0] = 0.25;
    maxBoundary[1] = 0.5;
    maxBoundary[2] = 0.75;
    maxBoundary[3] = 1.0;

    minBoundary[4] = 0.0;
    minBoundary[5] = 0.25;
    minBoundary[6] = 0.5;
    minBoundary[7] = 0.75;

    maxBoundary[4] = 0.25;
    maxBoundary[5] = 0.5;
    maxBoundary[6] = 0.75;
    maxBoundary[7] = 1.0;

    minBoundary[8] = 0.0;
    minBoundary[9] = 0.25;
    minBoundary[10] = 0.5;
    minBoundary[11] = 0.75;

    maxBoundary[8] = 0.25;
    maxBoundary[9] = 0.5;
    maxBoundary[10] = 0.75;
    maxBoundary[11] = 1.0;

    gEvaluator = &Evaluator2;
    searcher.Search(minBoundary, maxBoundary, gPassingVariables, 10000);

    delete[] minBoundary;
    delete[] maxBoundary;
}
